#include "math.h"
#include "vectormath.h"
#include "common.h"

namespace Tmpl {

// ===================================================================================
// Vec2
// ===================================================================================
float Vec2f::Length() const
{
	return sqrtf(x*x + y*y);
}

int Vec2i::Length() const
{
	return (int)sqrtf((float)(x*x + y*y));
}

// ===================================================================================
// Vec3
// ===================================================================================
float Vec3f::Length() const
{
	return sqrtf(x*x + y*y + z*z);
}

int Vec3i::Length() const
{
	return (int)sqrtf((float)(x*x + y*y + z*z));
}

// ===================================================================================
// Matrix3x3f
// ===================================================================================

void Matrix3x3f::Rotate(float rot)
{
	Identity();
	const float sz = sinf(rot * (PI/180.0f));
	const float cz = cosf(rot * (PI/180.0f));
	cell[0] = cz, cell[1] = sz, cell[3] = -sz, cell[4] = cz;
}

// ===================================================================================
// Matrix4x4f
// ===================================================================================

void Matrix4x4f::SetRotateX(float rotX)
{
	Identity();
	const float sx = sinf(rotX * (PI/180.0f));
	const float cx = cosf(rotX * (PI/180.0f));
	cell[5] = cx, cell[6] = sx, cell[9] = -sx, cell[10] = cx;
}

void Matrix4x4f::SetRotateY(float rotY)
{
	Identity();
	const float sy = sinf(rotY * (PI/180.0f));
	const float cy = cosf(rotY * (PI/180.0f));
	cell[0] = cy, cell[2] = -sy, cell[8] = sy, cell[10] = cy;
}

void Matrix4x4f::SetRotateZ(float rotZ)
{
	Identity();
	const float sz = sinf(rotZ * (PI/180.0f));
	const float cz = cosf(rotZ * (PI/180.0f));
	cell[0] = cz, cell[1] = sz, cell[4] = -sz, cell[5] = cz;
}

void Matrix4x4f::RotateX(float rotX)
{
	Matrix4x4 m;
	const float sx = sinf(rotX * (PI/180.0f));
	const float cx = cosf(rotX * (PI/180.0f));
	m.cell[5] = cx, m.cell[6] = sx, m.cell[9] = -sx, m.cell[10] = cx;
	Concatenate(m);
}

void Matrix4x4f::RotateY(float rotY)
{
	Matrix4x4 m;
	const float sy = sinf(rotY * (PI/180.0f));
	const float cy = cosf(rotY * (PI/180.0f));
	m.cell[0] = cy, m.cell[2] = -sy, m.cell[8] = sy, m.cell[10] = cy;
	Concatenate(m);
}

void Matrix4x4f::RotateZ(float rotZ)
{
	Matrix4x4 m;
	const float sz = sinf(rotZ * (PI/180.0f));
	const float cz = cosf(rotZ * (PI/180.0f));
	m.cell[0] = cz, m.cell[1] = sz, m.cell[4] = -sz, m.cell[5] = cz;
	Concatenate(m);
}

void Matrix4x4f::SetPerspective(float fov, float aspect, float zNear, float zFar)
{
	for (int i=0; i<16; ++i) cell[i] = 0;
	const float f = 1.0f / tan( fov * PI / 360.0f ); //1.0f / tan( fov * 0.5f );
	cell[0] = f / aspect;
	cell[5] = f;
	cell[10] = (zFar + zNear) / (zNear - zFar);
	cell[11] = -1.0f;
	cell[14] = (2.0f * zFar * zNear) / (zNear - zFar);
}

// ===================================================================================
// Quaternionf
// ===================================================================================
float Quaternionf::Length() const
{
	return sqrtf(x*x + y*y + z*z + w*w);
}

void Quaternionf::SetMatrix(const Matrix4x4f& m)
{
	float t = m[0] + m[5] + m[10] + 1.0f;

	if( t > 0.0f )
	{
		float s = 0.5f / sqrtf(t);
		w = 0.25f / s;
		x = ( m[9] - m[6] ) * s;
		y = ( m[2] - m[8] ) * s;
		z = ( m[4] - m[1] ) * s;
	}
	else
	{
		if ( m[0] > m[5] && m[0] > m[10])
		{
			float s = sqrtf( 1.0f + m[0] - m[5] - m[10] ) * 2.0f;
			x = 0.5f * s;
			y = (m[1] + m[4] ) / s;
			z = (m[2] + m[8] ) / s;
			w = (m[6] + m[9] ) / s;
		}
		else if ( m[5] > m[10] )
		{
			float s = sqrtf( 1.0f + m[5] - m[0] - m[10] ) * 2.0f;
			x = (m[1] + m[4] ) / s;
			y = 0.5f / s;
			z = (m[6] + m[9] ) / s;
			w = (m[2] + m[8] ) / s;
		}
		else
		{
			float s = sqrtf( 1.0f + m[10] - m[0] - m[5] ) * 2.0f;
			x = (m[2] + m[8] ) / s;
			y = (m[6] + m[9] ) / s;
			z = 0.5f / s;
			w = (m[1] + m[4] ) / s;
		}
	}

	Normalize();
}
void Quaternionf::SetRotation(float rotX, float rotY, float rotZ)
{
	float angle;

	angle = (rotX * (PI / 180.0f)) * 0.5f;
	const float sr = sin(angle);
	const float cr = cos(angle);

	angle = (rotY * (PI / 180.0f)) * 0.5f;
	const float sp = sin(angle);
	const float cp = cos(angle);

	angle = (rotZ * (PI / 180.0f)) * 0.5f;
	const float sy = sin(angle);
	const float cy = cos(angle);

	const float cpcy = cp * cy;
	const float spcy = sp * cy;
	const float cpsy = cp * sy;
	const float spsy = sp * sy;

	x = sr * cpcy - cr * spsy;
	y = cr * spcy + sr * cpsy;
	z = cr * cpsy - sr * spcy;
	w = cr * cpcy + sr * spsy;

	Normalize();
}

Vec3f Quaternionf::GetRotation() const
{
	Vec3f euler;

	const float sqw = w*w;
	const float sqx = x*x;
	const float sqy = y*y;
	const float sqz = z*z;
	const float test = 2.0f * (y*w - x*z);

	if (fabs(test-1.0f) < 0.000001f)
	{
		// heading = rotation about z-axis
		euler.z = -2.0f * atan2f(x, w);
		// bank = rotation about x-axis
		euler.x = 0;
		// attitude = rotation about y-axis
		euler.y = PI / 2.0f;
	}
	else if (fabs(test+1.0f) < 0.000001f)
	{
		// heading = rotation about z-axis
		euler.z = 2.0f * atan2f(x, w);
		// bank = rotation about x-axis
		euler.x = 0;
		// attitude = rotation about y-axis
		euler.y = PI / -2.0f;
	}
	else
	{
		// heading = rotation about z-axis
		euler.z = atan2f(2.0f * (x*y + z*w), (sqx - sqy - sqz + sqw));
		// bank = rotation about x-axis
		euler.x = atan2f(2.0f * (y*z +x*w), (-sqx - sqy + sqz + sqw));
		// attitude = rotation about y-axis
		euler.y = asinf( Clamp(-1.0f, 1.0f, test) );
	}
	return euler * (180.0f/PI);
}
void Quaternionf::GetAxisAngle(Vec3f& axis, float& angle)
{
	/*
	angle = 2.0f * acosf(w);
	float scale = sqrtf( 1.0f - w * w );
	if (fabs(scale) < 0.0005f) scale = 1.0f;
	float iScale = 1.0f / scale;
	axis.x = x * iScale;
	axis.y = y * iScale;
	axis.z = z * iScale;
	*/

	const float scale = sqrtf(x*x + y*y + z*z);
	if (fabs(scale) < 0.001f || w > 1.0f || w < -1.0f)
	{
		angle = 0.0f;
		axis.x = 0.0f;
		axis.y = 1.0f;
		axis.z = 0.0f;
	}
	else
	{
		const float iScale = 1.0f / scale;
		angle = 2.0f * acosf(w);
		axis.x = x * iScale;
		axis.y = y * iScale;
		axis.z = z * iScale;
	}
}
void Quaternionf::SetAxisAngle(Vec3f axis, float angle)
{
	const float fHalfAngle = 0.5f * angle;
	const float fSin = sinf(fHalfAngle);
	w = cosf(fHalfAngle);
	x = fSin * axis.x;
	y = fSin * axis.y;
	x = fSin * axis.z;
}

void Quaternionf::SetRotationFromTo(Vec3f from, Vec3f to)
{
	Vec3f v0 = from.Normalized();
	Vec3f v1 = to.Normalized();

	const float d = v0.Dot(v1);
	if (d > 0.99999f) // If dot == 1, vectors are the same
	{
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		w = 1.0f;
	}
	else if (d < -0.99999f) // exactly opposite
	{
		Vec3f axis(1.0f, 0.0f, 0.0f);
		axis = axis.Cross(Vec3f(x,y,z));
		if (axis.SqrLength() < 0.0001f)
		{
			axis = Vec3f(0.0f,1.0f,0.0f).Cross(Vec3f(x,y,z));
		}
		SetAxisAngle(axis, PI);
	}
	else
	{
		const float s = sqrtf( (1.0f+d)*2.0f );
		const Vec3f c = v0.Cross(v1) * (1.0f / s);
		x = c.x;
		y = c.y;
		z = c.z;
		w = s * 0.5f;
	}
}

Matrix4x4f Quaternionf::GetMatrix() const
{
	Matrix4x4f m;

	float xx = x * x;
    float xy = x * y;
    float xz = x * z;
    float xw = x * w;
    float yy = y * y;
    float yz = y * z;
    float yw = y * w;
    float zz = z * z;
    float zw = z * w;

    m[0]  = 1.0f - 2.0f * ( yy + zz );
    m[1]  = 2.0f * ( xy - zw );
    m[2]  = 2.0f * ( xz + yw );
	m[3]  = 0.0f;

    m[4]  = 2.0f * ( xy + zw );
    m[5]  = 1.0f - 2.0f * ( xx + zz );
    m[6]  = 2.0f * ( yz - xw );
	m[7]  = 0.0f;

    m[8]  = 2.0f * ( xz - yw );
    m[9]  = 2.0f * ( yz + xw );
    m[10] = 1.0f - 2.0f * ( xx + yy );
	m[11] = 0.0f;

	m[12] = 0.0f;
	m[13] = 0.0f;
	m[14] = 0.0f;
	m[15] = 1.0f;

	return m;
}

}; // namespace Tmpl